/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */
#ifndef  MRPT_MATH_WRAP2PI_H
#define  MRPT_MATH_WRAP2PI_H

#define _USE_MATH_DEFINES // (For VS to define M_PI, etc. in cmath)
#include <cmath>
#include <cstddef>  // size_t

namespace mrpt
{
        namespace math
    {
        /** \addtogroup container_ops_grp
            * @{ */

        /** Modifies the given angle to translate it into the [0,2pi[ range.
            * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long double.
            * \sa wrapToPi, wrapTo2Pi, unwrap2PiSequence
            */
        template <class T>
        inline void wrapTo2PiInPlace(T &a)
        {
            bool was_neg = a<0;
            a = fmod(a, static_cast<T>(2.0*M_PI) );
            if (was_neg) a+=static_cast<T>(2.0*M_PI);
        }

        /** Modifies the given angle to translate it into the [0,2pi[ range.
            * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long double.
            * \sa wrapToPi, wrapTo2Pi, unwrap2PiSequence
            */
        template <class T>
        inline T wrapTo2Pi(T a)
        {
            wrapTo2PiInPlace(a);
            return a;
        }

        /** Modifies the given angle to translate it into the ]-pi,pi] range.
            * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long   double.
            * \sa wrapTo2Pi, wrapToPiInPlace, unwrap2PiSequence
        */
        template <class T>
        inline T wrapToPi(T a)
        {
            return wrapTo2Pi( a + static_cast<T>(M_PI) )-static_cast<T>(M_PI);
        }

        /** Modifies the given angle to translate it into the ]-pi,pi] range.
            * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long double.
            * \sa wrapToPi,wrapTo2Pi, unwrap2PiSequence
            */
        template <class T>
        inline void wrapToPiInPlace(T &a)
        {
            a = wrapToPi(a);
        }

        /** Modify a sequence of angle values such as no consecutive values have a jump larger than PI in absolute value.
            * \sa wrapToPi
            */
        template <class VECTOR>
        void unwrap2PiSequence(VECTOR &x)
        {
            const size_t N=x.size();
            for (size_t i=0;i<N;i++)
            {
                mrpt::math::wrapToPiInPlace(x[i]); // assure it's in the -pi,pi range.
                if (!i) continue;
                double Ap = x[i]-x[i-1];
                if (Ap>M_PI)  x[i]-=2.*M_PI;
                if (Ap<-M_PI) x[i]+=2.*M_PI;
            }
        }

        /** Computes the shortest angular increment (or distance) between two planar orientations,
            * such that it is constrained to [-pi,pi] and is correct for any combination of angles (e.g. near +-pi)
            * Examples: angDistance(0,pi) -> +pi; angDistance(pi,0) -> -pi;
            *           angDistance(-3.1,3.1) -> -0.08; angDistance(3.1,-3.1) -> +0.08;
            * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long double.
            * \sa wrapToPi, wrapTo2Pi
            */
        template <class T>
        inline T angDistance(T from, T to)
        {
            wrapToPiInPlace(from);
            wrapToPiInPlace(to);
            T d = to-from;
            if (d>M_PI)  d-=2.*M_PI;
            else if (d<-M_PI) d+=2.*M_PI;
            return d;
        }

        /** @} */

    } // End of MATH namespace
} // End of namespace

#endif
